"""
latitude controller based on stanley algorithm
"""
import math
import carla

from agents.tools.misc import compute_magnitude_angle

class VehicleState():
    def __init__(self, car_location=carla.Location(0,0,0), yaw=0.0, v=0.0):
        self.location  = car_location
        self.v = v
        self.yaw = yaw

    def update(self, acc, delta, axis_l, dt):
        self.location.x += self.v * math.cos(self.yaw) * dt
        self.location.y += self.v * math.sin(self.yaw) * dt
        self.yaw += self.v / axis_l * math.tan(delta) * dt
        self.v += acc * dt

class StanleyController():
    def __init__(self, car_location=carla.Location(0,0,0), yaw=0, v=0, dt=0.01, k=0.3):
        """
        @param car_location : carla.Location
        @param yaw : yaw of vehicle
        @param v : speed of vehicle
        @param dt : period of control
        @param k : parameter of stanley control
        """
        self._dt = dt
        self._k = k
        self._vehicle_state = VehicleState(car_location, yaw, v)
        pass

    def run_step(self, transform, max_steer_angle = 35.0):
        """
        @param transform : 路径期望点
        @ max_steer_angle: 前轮最大转向角
        """
        #此路线节点期望的航向角减去当前车辆航向角(航向偏差)，然后再加上横向偏差角即match.atan()得到的值
        #得到的delta即为控制车辆方向盘的控制量 
        wp_yaw = transform.rotation.yaw*math.pi/180.0 # 道路朝向，即期望朝向

        car_yaw = self._vehicle_state.yaw # 车辆当前朝向
        
        # 计算横向误差(当前路点处的误差)
        error, angle = compute_magnitude_angle(transform.location, self._vehicle_state.location, self._vehicle_state.yaw)

        # 如果路点在车辆左侧，误差为正，反之为负
        # 计算车辆前轴中心到最近路点的角度
        angle = math.atan2(transform.location.y - self._vehicle_state.location.y, transform.location.x - self._vehicle_state.location.x) # -pi ~ pi
        diff_angle = angle - car_yaw
        if diff_angle >= math.pi:
            diff_angle -= 2*math.pi
        elif diff_angle < -math.pi:
            diff_angle += 2*math.pi
        if 0> diff_angle > -math.pi:
            error = -1.0*error
        # print('error_dist={}, speed={}'.format(error, self._vehicle_state.v))

        # TODO 之后加入预瞄点，根据预瞄点修改k的值
        if self._vehicle_state.v > 0.1:
            error_yaw = math.atan2(self._k*error, self._vehicle_state.v)
        else:
            error_yaw = 0

        delta = wp_yaw - car_yaw +  error_yaw
        while delta > math.pi:
            delta -= math.pi * 2.0
        while delta < -math.pi:
            delta += math.pi * 2.0

        steer = delta * 180.0 / math.pi / max_steer_angle
        return steer, error
